
import lejos.nxt.LCD;

public class NxtStart {
	
	/**
	 * TFL115 Group 4
	 * Wrapper for the Mindstorm project
	 * Runs everything according to spec  
	 * ---------------------------------
	 * 
	 * 
	 * Define surface light threshold 
	 * When the object is empty the value is ~58
	 */ 
	public static final int threshold = 55;
	
	/** 
	 * Number of rectangles in which we divide the object
	 * Remember to use only odd values, otherwise the arm
	 * will be left in wrong position 
	 * The higher the value the more accurate result	
	 */
	public static final int rectangles = 21; 
	
	public static void main(String[] args) {
		MotorControl mc = new MotorControl(threshold);
		ScanReg scan = new ScanReg();
		int scanDirection = 1;
		
		// Begin scan
		for(int i=0; i<=rectangles; i++) {
			// Motor 1: Wheels 
			mc.startMotor(1, 0, 15, 500, 1);
			
			// Motor 3: Scanner Arm
			mc.startMotor(3, scanDirection, 100, 7000, 1);
			
			// Gather all scan data 
			scan.add(i, mc.getScanHits());
			
			// Change direction of the scanner arm
			scanDirection = (scanDirection == 1) ? 0 : 1;
		}
		// </end>
		
		// Move the robot over to the blank sheet
		mc.startMotor(1, 0, 50, 4000, 1);
		
		// Begin reproduce
		for(int i=0; i<=rectangles; i++) {
			mc.startMotor(1, 0, 15, 500, 1);
			// Loop over each key/value pair
			if(scan.get(i).size() > 0) {
				for(int j=0; j<scan.get(i).size(); j++) {
					// Display array size
					LCD.drawInt(scan.get(i).size(), 0, 5);	
					/**
					 * Used to check from which side the scan was found
					 * so we can adjust to the difference in time delay
					 */
					if(i % 2 == 0) {
						mc.startMotor(3, 1, 100, (scan.get(i).get(j) - 700), 1);
						mc.startMotor(2, 0, 60, 1000, 1);
						mc.startMotor(2, 1, 60, 1000, 1);
						mc.startMotor(3, 0, 100, (scan.get(i).get(j) - 700), 1);
					} else {
						mc.startMotor(3, 1, 100, 8000, 1);
						mc.startMotor(3, 0, 100, (scan.get(i).get(j) + 720), 1);
						mc.startMotor(2, 0, 60, 1000, 1);
						mc.startMotor(2, 1, 60, 1000, 1);
						mc.startMotor(3, 1, 100, (scan.get(i).get(j) + 720), 1);
						mc.startMotor(3, 0, 100, 7000, 1);
					}
					
				}
			}
		}
		// </end>
	}
}
